#pragma once

#include "cartographer/rcm/imu/preintegrate.h"
#include "cartographer/rcm/imu/filter.h"
#include "cartographer/rcm/match/helper.h"

namespace cartographer
{
    struct FilterFactor
    {
        common::Time time;
        double averageOccupyProb;
        Eigen::Vector3d threshAcc;
        Eigen::Vector3d threshGyro;
    };

    class PoseIntegrator
    {
    public:
        using Ptr = std::shared_ptr<PoseIntegrator>;

        PoseIntegrator()
        {
        }

        void SetFilter(double threshTime)
        {
            filter_ = std::make_shared<ImuExponentialFilter>(threshTime);
        }

        void SetIntegrator(const ImuPreintegrate::Ptr &integrator)
        {
            integrator_ = integrator;
        }

        void UpdateFilter(const common::Time &time);

        PVR Predict(const PVR &pvrLast, const ImuBias &biasNew);

        int Integrate(const ImuMeasSet &imuSet, const common::Time &timeStart, const common::Time &timeEnd);

    private:
        common::Time timeStart_;
        common::Time timeEnd_;
        std::queue<FilterFactor> filterFactors_;
        ImuExponentialFilter::Ptr filter_ = nullptr;
        ImuPreintegrate::Ptr integrator_ = nullptr;
    };
} // namespace cartographer